import { GridMap } from "./Grid";
import { AstarNode } from "./Node";

export class HeapSort {
	private array: AstarNode[] = [];
	private currentIndex = 0;
	constructor() {
		this.array = new Array<AstarNode>();
		this.currentIndex = 0;
	}
	public getList() {
		return this.array;
	}
	public getSize() {
		return this.currentIndex;
	}
	public has(node: AstarNode): boolean {
		for (let index = 0; index < this.array.length; index++) {
			const element = this.array[index];
			if (node == element) {
				return true;
			}
		}
		return false;
	}
	public insertSort(source: AstarNode) {
		this.array[this.currentIndex] = source;
		this.tickedUp(this.currentIndex++);
	}
	public getMin() {
		let minNode = this.array[0];
		this.array[0] = this.array[--this.currentIndex];
		this.trickleDown(0);
		return minNode;
	}
	public clear() {
		for (const iterator of this.array) {
			iterator.parent = null;
		}
		this.currentIndex = 0;
		this.array.length = 0;
	}
	private trickleDown(index) {
		let temp = this.array[index];
		let minChildIndex;
		while (index < this.currentIndex / 2) {
			let leftChildIndex = 2 * index + 1;
			let rightChildIndex = leftChildIndex + 1;
			//右子节点存在，且小于左子节点
			if (rightChildIndex < this.currentIndex && this.array[leftChildIndex].f > this.array[rightChildIndex].f) {
				minChildIndex = rightChildIndex;
			}
			else {
				minChildIndex = leftChildIndex;
			}
			if (temp.f <= this.array[minChildIndex].f) {
				break;
			}
			this.array[index] = this.array[minChildIndex];
			index = minChildIndex;
		}
		this.array[index] = temp;
	}
	private tickedUp(index) {
		if (index == 0) {
			return;
		}
		let parentIndex = Math.floor((index - 1) / 2);//父节点索引

		let temp = this.array[index];
		while (index > 0 && this.array[parentIndex].f > temp.f) {
			this.array[index] = this.array[parentIndex];
			index = parentIndex;
			parentIndex = Math.floor((parentIndex - 1) / 2);
		}
		this.array[index] = temp;
	}
}
export class AStar {
	public static instance = new AStar();
	private _open: Array<AstarNode> = [];               //待考察表
	private _closed: Array<AstarNode> = [];             //已考察表
	private _endNode: AstarNode;                  //终点Node
	private _startNode: AstarNode;                //起点Node
	private _path: Array<any>;               //保存路径
	private _heuristic: Function;            //寻路算法
	private _straightCost: number = 1.0;     //上下左右走的代价
	private _diagCost: number = Math.SQRT2;  //斜着走的代价 


	public constructor() {
		//this._heuristic = this.manhattan;  
		//this._heuristic = this.euclidian;
		this._heuristic = this.diagonal;
	}

	//寻路
	public findPath(startPos,endPos,): AstarNode[] {
		this._startNode = GridMap.instance.getNode(startPos.x,startPos.y);
		this._endNode = GridMap.instance.getNode(endPos.x,endPos.y);
		for (const iterator of this._open) {
			iterator.parent = null;
		}
		for (const iterator of this._closed) {
			iterator.parent = null;
		}
		this._open = [];
		this._closed = [];

		this._startNode.g = 0;
		this._startNode.h = this._heuristic(this._startNode);
		this._startNode.f = this._startNode.g + this._startNode.h;

		return this.search();
	}

	//查找路径
	public search(): AstarNode[] {
		var node: AstarNode = this._startNode;
		while (node != this._endNode) {
			var startX = Math.max(0, node.x - 1);
			var endX = Math.min(GridMap.instance.numCols - 1, node.x + 1);
			var startY = Math.max(0, node.y - 1);
			var endY = Math.min(GridMap.instance.numRows - 1, node.y + 1);

			for (var i = startX; i <= endX; i++) {
				for (var j = startY; j <= endY; j++) {
					//不让斜着走
					// if (i != node.x && j != node.y) {
					// 	continue;
					// }

					var test: AstarNode = GridMap.instance.getNode(i, j);
					if (test == node ||
						!test.walkable ||
						!GridMap.instance.getNode(node.x, test.y).walkable ||
						!GridMap.instance.getNode(test.x, node.y).walkable) {
						continue;
					}

					var cost: number = this._straightCost;
					if (!((node.x == test.x) || (node.y == test.y))) {
						cost = this._diagCost;
					}
					var g = node.g + cost * test.costMultiplier;
					var h = this._heuristic(test);
					var f = g + h;
					if (this.isOpen(test) || this.isClosed(test)) {
						if (test.f > f) {
							test.f = f;
							test.g = g;
							test.h = h;
							test.parent = node;
						}
					}
					else {
						test.f = f;
						test.g = g;
						test.h = h;
						test.parent = node;
						this._open.push(test);
					}
				}
			}
			this._closed.push(node);
			if (this._open.length == 0) {
				console.log("AStar >> no path found");
				return null;
			}

			let openLen = this._open.length;
			for (let m = 0; m < openLen; m++) {
				for (let n = m + 1; n < openLen; n++) {
					if (this._open[m].f > this._open[n].f) {
						let temp = this._open[m];
						this._open[m] = this._open[n];
						this._open[n] = temp;
					}
				}
			}

			node = this._open.shift() as AstarNode;
		}
		this.buildPath();
		console.log("end");
		
		return this._path;
	}

	//获取路径
	private buildPath(): void {
		this._path = new Array();
		var node: AstarNode = this._endNode;
		this._path.push(node);
		while (node != this._startNode) {
			node = node.parent;
			this._path.unshift(node);
		}
	}

	public get path() {
		return this._path;
	}

	//是否待检查
	private isOpen(node: AstarNode): boolean {
		for (var i = 0; i < this._open.length; i++) {
			if (this._open[i] == node) {
				return true;
			}
		}
		return false;
	}

	//是否已检查
	private isClosed(node: AstarNode): boolean {
		for (var i = 0; i < this._closed.length; i++) {
			if (this._closed[i] == node) {
				return true;
			}
		}
		return false;
	}

	//曼哈顿算法
	private manhattan(node: AstarNode) {
		return Math.abs(node.x - this._endNode.x) * this._straightCost + Math.abs(node.y + this._endNode.y) * this._straightCost;
	}


	private euclidian(node: AstarNode) {
		var dx = node.x - this._endNode.x;
		var dy = node.y - this._endNode.y;
		return Math.sqrt(dx * dx + dy * dy) * this._straightCost;
	}

	private diagonal(node: AstarNode) {
		var dx = Math.abs(node.x - this._endNode.x);
		var dy = Math.abs(node.y - this._endNode.y);
		var diag = Math.min(dx, dy);
		var straight = dx + dy;
		return this._diagCost * diag + this._straightCost * (straight - 2 * diag);
	}

	public get visited() {
		return this._closed.concat(this._open);
	}

}